/*
 * libwsnpkt.c
 *
 *  Created on: Jul 24, 2010
 *      Author: carlo
 */


#include "libwsnpkt.h"

pkt_size_t pkt_init(unsigned char *pkt_buf, pkt_size_t dim, pkt_size_t pr_dim)
{
	static pkt_seq_t seq = 0;
	pkt_size_t last = sizeof(pkt_size_t) + sizeof(pkt_mac_t) + sizeof(pkt_seq_t) + pr_dim;
	pkt_size_t pkt_len = sizeof(pkt_size_t) + sizeof(pkt_mac_t) + sizeof(pkt_seq_t) + sizeof(pkt_term_t);
	pkt_size_t i;

	seq++;

	if(dim < pkt_len)
		return(RET_ERR_SIZE);

	for(i = 0; i < dim; i++)
		*(pkt_buf + i) = (i < pr_dim) ? PRE_BYTE : 0x00;


	*(pkt_buf + pr_dim) = pkt_len;
	*(pkt_buf + last) = (pkt_term_t) TERM_BYTE;

	return(pkt_len + pr_dim);
}

pkt_size_t pkt_set_sensor(unsigned char *pkt_buf, pkt_size_t dim, sens_id_t id, char *data, sens_data_len_t n_bytes)
{
	unsigned char *p = pkt_buf;
	sens_data_len_t i;
	pkt_size_t pkt_len;
	pkt_size_t last;
	sens_field_t *s_field;
	pkt_preamb_t l_pre = 0;

	while(*p == PRE_BYTE)
		p++, l_pre++;

	pkt_len = *p + sizeof(sens_head_t) + n_bytes;
	last = *p - sizeof(pkt_term_t);

	if(dim < pkt_len || pkt_len > MAX_LEN_PKT)
		return(RET_ERR_SIZE);

	s_field = (sens_field_t *) (p + last);
	s_field->header = HEAD_S(id,n_bytes);

	for(i = 0; i < n_bytes; i++)
#ifdef BIG_ENDIAN_HOST
		*(s_field->data + i) = *(data + (n_bytes - 1) - i);
#else
		*(s_field->data + i) = *(data + i);
#endif

	*(p + pkt_len - sizeof(pkt_term_t)) = (pkt_term_t) TERM_BYTE;
	*p = pkt_len;

	return(pkt_len + l_pre);
}

pkt_size_t pkt_set_mac(unsigned char *pkt_buf, pkt_mac_t mac, pkt_size_t len)
{
	unsigned char *p = pkt_buf;
	pkt_head_t *head;
	pkt_size_t i;
	pkt_preamb_t l_pre = 0;

	while(*p == PRE_BYTE)
		p++, l_pre++;

	head = (pkt_head_t *) p;

	if (len != MAC_SIZE)
		return(RET_ERR_SIZE);

	for(i = 0; i < MAC_SIZE; i++)
#ifdef BIG_ENDIAN_HOST
		*(head->mac + i) = *(mac + (len - 1) - i);
#else
		*(head->mac + i) = *(mac + i);
#endif

	return(*p + l_pre);
}

pkt_size_t pkt_get_mac(unsigned char *pkt_buf, pkt_mac_t mac, pkt_size_t len)
{
	unsigned char *p = pkt_buf;
	pkt_head_t *head;
	pkt_size_t i;
	pkt_preamb_t l_pre = 0;

	while(*p == PRE_BYTE)
		p++, l_pre++;

	head = (pkt_head_t *) p;

	if (len != MAC_SIZE)
		return(RET_ERR_SIZE);

	for(i = 0; i < MAC_SIZE; i++)
#ifdef BIG_ENDIAN_HOST
		 *(mac + (MAC_SIZE - 1) - i) = *(head->mac + i);
#else
		 *(mac + i) = *(head->mac + i);
#endif

	return(*p + l_pre);
}


pkt_size_t pkt_get_size(unsigned char *pkt_buf)
{
	unsigned char *p = pkt_buf;
	pkt_preamb_t l_pre = 0;

	while(*p == PRE_BYTE)
		p++, l_pre++;

	return(*p + l_pre);
}

pkt_size_t pkt_get_sensor(unsigned char *pkt_buf, sens_id_t id, char *data, sens_data_len_t n_bytes)
{
	unsigned char *p = pkt_buf;
	sens_field_t *s_field ;
	pkt_size_t last;
	pkt_size_t i;
	unsigned char *s;
	pkt_preamb_t l_pre = 0;


	while(*p == PRE_BYTE)
		p++, l_pre++;

	last = *p;
	s_field = (sens_field_t *) (p + sizeof(pkt_size_t) + sizeof(pkt_mac_t) + sizeof(pkt_seq_t));

	if((p + last) <= ((unsigned char *) s_field + sizeof(sens_head_t))) // TODO check <=
			return(RET_ERR_NOTFOUND);

	while(EXTR_S_ID(s_field->header) != id) {
		s_field = (sens_field_t *) ((unsigned char *) s_field + (sizeof(sens_head_t) + EXTR_S_LEN(s_field->header)));
		if((p + last) <= (unsigned char *) s_field)
			return(RET_ERR_NOTFOUND);
	}

	if(n_bytes < EXTR_S_LEN(s_field->header))
		return(RET_ERR_SIZE);

	for(i = 0; i < EXTR_S_LEN(s_field->header); i++)
#ifdef BIG_ENDIAN_HOST
		*(data + (EXTR_S_LEN(s_field->header) - 1) - i) = *(s_field->data + i);
#else
		*(data + i) = *(s_field->data + i);
#endif

	*p = last - (sizeof(sens_head_t) + EXTR_S_LEN(s_field->header));

	s  = ((unsigned char *) s_field + (sizeof(sens_head_t) + EXTR_S_LEN(s_field->header)));
	for(i = 0; s != (p + last); i++, s++)
		*((unsigned char *)s_field + i) =  *(s);
	s = ((unsigned char *) s_field + i);


	while(s != (p + last))
		*s = 0, s++;


	return(*p + l_pre);
}

pkt_seq_t pkt_get_seq(unsigned char *pkt_buf)
{
	unsigned char *p = pkt_buf;
	pkt_head_t *head;

	while(*p == PRE_BYTE)
		p++;

	head = (pkt_head_t *) p;

	return(head->seq);
}

pkt_size_t pkt_set_seq(unsigned char *pkt_buf, pkt_seq_t seq)
{
	unsigned char *p = pkt_buf;
	pkt_head_t *head;
	pkt_preamb_t l_pre = 0;

	while(*p == PRE_BYTE)
		p++, l_pre;

	head = (pkt_head_t *) p;

	head->seq = seq;
	return(*p + l_pre);
}

char pkt_get_sensors_list(unsigned char *pkt_buf, unsigned char *ext_buf, pkt_size_t dim)
{
	unsigned char *p = pkt_buf;
	pkt_size_t last;
	unsigned char cnt_dim = 0;
	sens_field_t *s_field;

	while(*p == PRE_BYTE)
		p++;

	last = *p;
	s_field = (sens_field_t *) (p + sizeof(pkt_size_t) + sizeof(pkt_mac_t) + sizeof(pkt_seq_t));

	while((p + last) != ((unsigned char *) s_field + sizeof(pkt_term_t))) {
		ext_buf[cnt_dim] = s_field->header;
		if(cnt_dim == dim)
			return(RET_ERR_SIZE);
		cnt_dim++;
		s_field = (sens_field_t *) ((unsigned char *) s_field + (sizeof(sens_head_t) + EXTR_S_LEN(s_field->header)));
	}

	return(cnt_dim);
}









